//
//  RobotManager.swift
//  Urso
//
//  Created by Cosmetic Cygia on 2024/6/13.
//

import Foundation

protocol RobotController {
    var ipAddress: String { get set }
    var port: Int { get }
    var timeout: Double { get set }
    init(ipAddress: String, port: Int)
}

class InovanceRobotController : RobotController {
    
    
    var onMessageChanged: ((_ response: String) -> Void)?
    
    var ipAddress: String
    
    var port: Int
    
    var timeout: Double
    
    var stream:URLSessionStreamTask?
    
    required init(ipAddress: String, port: Int) {
        self.ipAddress = ipAddress
        self.port = port
        self.timeout = 10
        
   /*     if (stream != nil) {
            stream!.closeWrite()
            stream!.closeRead()
        }*/
        
        stream = URLSession.shared.streamTask(withHostName: ipAddress, port: port)
        if (stream != nil) {
     //       deprecateStreamRead(stream: stream!)
        }
    }
    
    //220
    func getPermit(escape: ((_ errorCode: Int, _ isSuccess: Bool) -> Void)?) {
        if (stream == nil) {
            escape!(-1, false)
        } else {
            let stringToSend = "@@AcqPermit$$"
            ELog.d(String(describing: type(of: self)),#function, "case4", #line)
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false)
                    } else {
                        if (response.starts(with: "##ok$$")) {
                            escape!(0, true)
                            
                        } else {
                            escape!(-3 , false)
                        }
                    }
                }
            })
        }
    }
    
    
    //220
    func getPermitForcibly(escape: ((_ errorCode: Int, _ isSuccess: Bool) -> Void)?) {
        if (stream == nil) {
            escape!(-1, false)
        } else {
            let stringToSend = "@@AcqPermit forcibly$$"
            ELog.d(String(describing: type(of: self)),#function, "case4", #line)
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false)
                    } else {
                        if (response.starts(with: "##ok$$")) {
                            escape!(0, true)
                            
                        } else {
                            escape!(-3 , false)
                        }
                    }
                }
            })
        }
    }
    
    //221
    func removePermit(escape: ((_ errorCode: Int, _ isSuccess: Bool) -> Void)?) {
        if (stream == nil) {
            escape!(-1, false)
        } else {
            let stringToSend = "@@RemovePermit$$"
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false)
                    } else {
                        if (response.starts(with: "##ok$$")) {
                            escape!(0, true)
                        } else {
                            escape!(-3 , false)
                        }
                    }
                }
            })
        }
    }
    
    // 223
    func login(mode: Int, password: String, escape: ((_ errorCode: Int, _ isSuccess: Bool) -> Void)? ) {
        if (stream == nil) {
            escape!(-1, false)
        } else {
            let stringToSend = "@@UserLogin \(mode) " + password + "$$"
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false)
                    } else {
                        if (response.starts(with: "##ok$$")) {
                            escape!(0, true)
                        } else {
                            escape!(-3 , false)
                        }
                    }
                }
            })
        }
    }
    //224
    func logout(escape: ((_ errorCode: Int, _ isSuccess: Bool) -> Void)?) {
        if (stream == nil) {
            escape!(-1, false)
        } else {
            let stringToSend = "@@UserLogout$$"
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false)
                    } else {
                        if (response.starts(with: "##ok$$")) {
                            escape!(0, true)
                        } else {
                            escape!(-3 , false)
                        }
                    }
                }
            })
        }
    }
    
    //3
    func setEmergencyStopStatus(status: Bool, escape: ((_ errorCode: Int, _ isSuccess: Bool) -> Void)? ) {
        if (stream == nil) {
            escape!(-1, false)
        } else {
            let stringToSend = "@@EStop " + (status ? "ON": "OFF") + "$$"
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false)
                    } else {
                        if (response.starts(with: "##ok$$")) {
                            escape!(0, true)
                        } else {
                            escape!(-3 , false)
                        }
                    }
                }
            })
        }
    }
    
    //5
    func resetSystemError(escape: ((_ errorCode: Int, _ isSuccess: Bool) -> Void)?) {
        if (stream == nil) {
            escape!(-1, false)
        } else {
            let stringToSend = "@@ResetErr$$"
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false)
                    } else {
                        if (response.starts(with: "##ok$$")) {
                            escape!(0, true)
                        } else {
                            escape!(-3 , false)
                        }
                    }
                }
            })
        }
    }
    
    // 84
    func getEmergencyStopStatus(escape: ((_ errorCode: Int, _ isSuccess: Bool, _ status: Bool) -> Void)?) {
        if (stream == nil) {
            escape!(-1, false, false)
        } else {
            let stringToSend = "@@Get_EStopSts$$"
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false, false)
                    } else {
                        if (response.starts(with: "##=0$$")) {
                            escape!(0, true, false)
                        } else if (response.starts(with: "##=1$$")) {
                            escape!(0, true, true)
                        } else {
                            escape!(-3, false, false)
                        }
                    }
                }
            })
        }
    }
    // 67
    func getSystemErrorStatus(escape: ((_ errorCode: Int, _ isSuccess: Bool, _ status: Bool) -> Void)?) {
        if (stream == nil) {
            escape!(-1, false, false)
        } else {
            let stringToSend = "@@Get_SysErrSts$$"
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false, false)
                    } else {
                        if (response.starts(with: "##=0$$")) {
                            escape!(0, true, false)
                        } else if (response.starts(with: "##=1$$")) {
                            escape!(0, true, true)
                        } else {
                            escape!(-3, false, false)
                        }
                    }
                }
            })
        }
    }
    // 4
    func setMotorStatus(status: Bool, escape: ((_ errorCode: Int, _ isSuccess: Bool) -> Void)? ) {
        if (stream == nil) {
            escape!(-1, false)
        } else {
            let stringToSend = "@@Motor " + (status ? "ON": "OFF") + "$$"
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false)
                    } else {
                        if (response.starts(with: "##ok$$")) {
                            escape!(0, true)
                        } else {
                            escape!(-3 , false)
                        }
                    }
                }
            })
        }
    }
    
    
    // 15
    func setDo(point: Int, status: Bool, escape: ((_ errorCode: Int, _ isSuccess: Bool) -> Void)? ) {
        if (stream == nil) {
            escape!(-1, false)
        } else {
            let stringToSend = "@@Set_DO \(point) " + (status ? "1": "0") + "$$"
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false)
                    } else {
                        if (response.starts(with: "##ok$$")) {
                            escape!(0, true)
                        } else {
                            escape!(-3 , false)
                        }
                    }
                }
            })
        }
    }
    // 14
    private let dsmodeStatusString = ["ON", "OFF", "PAUSE", "CONTINUE"]
    func setDsmodeStatus(status: Int, escape: ((_ errorCode: Int, _ isSuccess: Bool) -> Void)? ) {
        if (stream == nil) {
            escape!(-1, false)
        } else if (!((status >= 0) && (status <= 3))) {
            escape!(-3, false)
        }
        else {
            let stringToSend = "@@Dsmode " + dsmodeStatusString[status] + "$$"
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false)
                    } else {
                        if (response.starts(with: "##ok$$")) {
                            escape!(0, true)
                        } else {
                            escape!(-3 , false)
                        }
                    }
                }
            })
        }
    }
    // 28
    func goHome0(escape: ((_ errorCode: Int, _ isSuccess: Bool) -> Void)?) {
        if (stream == nil) {
            escape!(-1, false)
        } else {
            let stringToSend = "@@Home 0$$"
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false)
                    } else {
                        if (response.starts(with: "##ok$$")) {
                            escape!(0, true)
                        } else {
                            escape!(-3 , false)
                        }
                    }
                }
            })
        }
    }
    
    func setVel(vel: Int, escape: ((_ errorCode: Int, _ isSuccess: Bool) -> Void)?) {
        if (stream == nil) {
            escape!(-1, false)
        } else if (!((vel >= 1) && (vel <= 100))) {
            
            escape!(-4, false)
        } else {
            let stringToSend = "@@Set_Vel \(vel)$$"
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false)
                    } else {
                        if (response.starts(with: "##ok$$")) {
                            escape!(0, true)
                        } else {
                            escape!(-3 , false)
                        }
                    }
                }
            })
        }
    }
    
    //31
    func movJGP(point: Int, vel: Int, accuracy: Int, escape: ((_ errorCode: Int, _ isSuccess: Bool) -> Void)?) {
        // 检查是否为有效数据流
        if (stream == nil) {
            escape!(-1, false)
        } else if (!((point >= 0) && (point <= 9999))) {
            escape!(-4, false)
        } else if  (!((vel >= 0) && (vel <= 100))) {
            
            escape!(-5, false)
        } else if  (!(((accuracy >= -2) && (accuracy <= 200)) || ((accuracy >= 1000) && (accuracy <= 1200)))) {
            
            escape!(-6, false)
        }else {
            let stringToSend = "@@MovJGP GP\(point) \(vel) \(accuracy)$$"
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false)
                    } else {
                        if (response.starts(with: "##ok$$")) {
                            escape!(0, true)
                        } else {
                            escape!(-3 , false)
                        }
                    }
                }
            })
        }
    }
    
    
    //32
    func movLGP(point: Int, vel: Int, accuracy: Int, escape: ((_ errorCode: Int, _ isSuccess: Bool) -> Void)?) {
        if (stream == nil) {
            escape!(-1, false)
        } else if (!((point >= 0) && (point <= 9999))) {
            escape!(-4, false)
        } else if  (!((vel >= 0) && (vel <= 100))) {
            
            escape!(-5, false)
        } else if  (!(((accuracy >= -2) && (accuracy <= 200)) || ((accuracy >= 1000) && (accuracy <= 1200)))) {
            
            escape!(-6, false)
        }else {
            let stringToSend = "@@MovLGP GP\(point) \(vel) \(accuracy)$$"
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false)
                    } else {
                        if (response.starts(with: "##ok$$")) {
                            escape!(0, true)
                        } else {
                            escape!(-3 , false)
                        }
                    }
                }
            })
        }
    }
    
    
    
    //33
    func movCGP(pointCenter: Int, pointDest: Int, vel: Int, accuracy: Int, escape: ((_ errorCode: Int, _ isSuccess: Bool) -> Void)?) {
        if (stream == nil) {
            escape!(-1, false)
        } else if (!((pointCenter >= 0) && (pointCenter <= 9999))) {
            escape!(-4, false)
            
        } else if (!((pointDest >= 0) && (pointDest <= 9999))) {
            escape!(-4, false)
        } else if  (!((vel >= 0) && (vel <= 100))) {
            
            escape!(-5, false)
        } else if  (!(((accuracy >= -2) && (accuracy <= 200)) || ((accuracy >= 1000) && (accuracy <= 1200)))) {
            
            escape!(-6, false)
        }else {
            let stringToSend = "@@MovCGP GP\(pointCenter) GP\(pointDest) \(vel) \(accuracy)$$"
            sendString(stream: self.stream!, stringToSend: stringToSend, timeout: self.timeout, escape: { isSuccess, response in
                self.onMessageChanged?("Send: " + stringToSend + "\nResponse: " + String(response))
                
                if (!isSuccess) {
                    escape!(-9999, false)
                } else {
                    if (response.count != 6) {
                        escape!(-2, false)
                    } else {
                        if (response.starts(with: "##ok$$")) {
                            escape!(0, true)
                        } else {
                            escape!(-3 , false)
                        }
                    }
                }
            })
        }
    }
}

